Motion estimation method and system for mobile body

ABSTRACT

A motion estimation method and system for a mobile body are provided. The method includes: obtaining magnetic field information from compass information of the mobile body; comparing the magnetic field of the mobile body with a predetermined value and determining whether a position of the mobile body belongs to a specific region according to the comparison result; and estimating a direction of the mobile body by determining whether a compass azimuth angle is used for direction estimation of the mobile body according to the determination result. The system, in which a gyro, odometers, and compasses are installed, comprises: a magnetic field calculator calculating the magnitudes of magnetic fields of the mobile body; a magnetic field comparator obtaining differences between the magnitudes of the magnetic fields and the magnitude of the geomagnetic field and comparing the differences with the first threshold value; a geomagnetic region determiner determining whether a position where the mobile body exists belongs to a region where the geomagnetism works according to the comparison result; and a moving direction estimator estimating a moving direction of the mobile body by determining whether or not to use azimuth angles of the compasses for direction estimation of the mobile body according to the determination result.

BACKGROUND OF THE INVENTION

This application claims the priority of Korean Patent Application No.2003-78873, filed on Nov. 8, 2003, in the Korean Intellectual PropertyOffice, the disclosure of which is incorporated herein in its entiretyby reference.

1. Field of the Invention

The present invention relates to motion estimation of a mobile body, andmore particularly, to a motion estimation method and system for a mobilebody using compass data.

2. Description of the Related Art

Pose estimation of a mobile body is achieved by estimating a positionand an orientation of the mobile body using an absolute sensor or arelative sensor. Pose estimation of a mobile body moving on a2-dimensional plane as shown in FIG. 1 is achieved by estimating theposition (x, y) and the direction θ of the mobile body. In this case,the said absolute sensor can measure the absolute position ororientation of the mobile body instead of its relative motion. A camera,a laser scanner, sonar sensors, a global positioning systems (GPS), or amagnetic compass can be used as an absolute sensor. On the other hand,the said relative sensor can measure a position or an orientation bymeasuring and integrating a relative increments or decrements value ofthe motion of the mobile body. A gyro, an accelerometer, and an odometer(an encoder attached to a motor) can be used as the relative sensor.

Some of the characteristics of the absolute sensors are as follows: thecamera is sensitive to a lighting status of surrounding environments andhas a high possibility of outputting unreliable data; the laser scannercan output reliable data but is very expensive, and if a number ofobstacles exist, it is difficult to measure a position or an orientationusing the laser scanner; the sonar has low data accuracy; the GPS can beused only outside and has low precision; and the compass has a highpossibility of being affected by a disturbance of magnetic fieldexisting indoors. On the other hand, since the relative sensors measureonly a variable value, an integration error is inevitably generated byintegrating the variable value, and a drift error characteristics of thegyro and the accelerometer cannot be avoided. In an embodiment of thepresent invention, the disadvantages described above are reduced bysensor fusion using a compass as an absolute sensor and a gyro and anodometer as relative sensors.

A conventional dead-reckoning pose estimation method is classified intoa method using only an odometer, a method using a gyro and an odometer,and a method using a compass and an odometer. The method using only theodometer is the simplest one. However, the method using only theodometer cannot cope with a slippage error, a bump collision, orkidnapping. Also, since errors are continuously accumulated in thismethod, an unbounded error exists.

To solve the problems described above, a method of performingdead-reckoning by using a gyro and an odometer has been developed.However, even though this method can obtain a more accurate result ascompared with the method using only the odometer, i.e., the encoder,since both the encoder and the gyro are relative sensors, boundednesscannot be still guaranteed in long-term. A method of stably detecting amoving direction in long-term by introducing the compass, which is theabsolute sensor, instead of the gyro also has been developed. However,since this method can easily be affected by a disturbance of magneticfield always existing in a home or office environment, it is difficultto practically use the method.

Recently, a method using a gyro, a compass, and an odometer together hasalso been suggested. Since this method simultaneously uses a gyro and acompass, mutual-aid between the gyro and the compass is possible.However, since most currently developed methods use the existing Kalmanfilter, which is a statistics-based sensor fusion method, variouslimitations originating from inherent demerit of Kalman filter exist.That is, since system noise and measurement noise must have Gaussianwhite noise characteristics for the application of Kalman filter, it isdifficult to deal with various kinds of situations such as a magneticfield, an obstacle, and a slippery floor, which don't show Gaussianwhite noise characteristics. Besides, exact information of all kinds ofmodels of sensors is necessary, and an assumption that error sources areuncorrelated to each other is also needed. However, practically, amethod and system meeting the requirements described above cannot berealized. Also, since the performance of the sensors are important forsensor fusion, expensive sensors are mostly used in this method.

SUMMARY OF THE INVENTION

The present invention provides a motion estimation method and system fora mobile body, which use sensor fusion to calculate pose information ofthe mobile body by accurately and robustly estimating an orientation ofthe mobile body.

The present invention also provides a computer readable medium havingrecorded thereon a computer readable program for performing the motionestimation method for a mobile body.

According to an aspect of the present invention, there is provided amotion estimation method for a mobile body, the method comprising:obtaining magnetic field information from the magnetic compassesattached to the mobile body; comparing the magnetic field of the mobilebody with a predetermined value and determining whether a position ofthe mobile body belongs to a specific region according to the comparisonresult; and estimating a moving direction of the mobile body bydetermining whether a compass azimuth angle is used for estimating theorientation of the mobile body according to the determination result.

Here, the predetermined value can be the magnitude of the geomagneticfield, and the specific region can be a region in which the Earth'smagnetic field works dominantly. The determination of whether a positionof the mobile body belongs to a specific region can comprise: if twocompasses are installed in the mobile body, comparing each of themagnitude of a magnetic field of the first compass and the magnitude ofa magnetic field of the second compass with the magnitude of thegeomagnetic field, dividing the comparison results into a case whereboth differences are less than a first threshold value, a case whereonly one of the two differences is less than the first threshold value,and a case where the two differences are not less than the firstthreshold value, and determining whether a position of the mobile bodybelongs to the specific region according to the comparison results.

The estimating of the moving direction of the mobile can comprise: if itis determined that the compass information is valid according as themobile body belongs to a specific region, obtaining a final compassazimuth angle and estimating a heading angle using a Kalman filter usingthe final compass azimuth angle as a measurement input; and if it isdetermined that the compass information is invalid according as themobile body does not belong to a specific region, comparing an angularvelocity of a gyro and an angular velocity of an odometer, and if thedifference is less than a second threshold value, estimating a headingdirection using the Kalman filter using a moving direction obtained bythe odometer as a measurement input, and if the difference is not lessthan the second threshold value, estimating the moving direction byintegrating the angular velocity of the gyro.

The estimating of the moving direction of the mobile body in the casewhere each of the magnitude of the magnetic field of the first compassand the magnitude of the magnetic field of the second compass iscompared with the magnitude of the geomagnetic field and the twodifferences are less than the first threshold value can comprise:obtaining a difference between an azimuth angle of the first compass andan azimuth angle of the second compass; if the difference between theazimuth angles is less than a third threshold value, obtaining a compassazimuth angle of the mobile body by varying a weight according todifferences between the magnitudes of the magnetic fields of the twocompasses and the magnitude of the geomagnetic field; and if thedifference between the azimuth angles is not less than the thirdthreshold value, obtaining a difference between an angular velocity ofthe gyro with respect to the mobile body and each angular velocity withrespect to the azimuth angles of the compasses and obtaining the compassazimuth angle of the mobile body according to the magnitudes of thedifferences.

The obtaining of the compass azimuth angle of the mobile body by varyinga weight can comprise: when Δθ_(c1) indicates the amount of how much anazimuth angle of the first compass is changed for a sampling period,Δθ_(c2) indicates the amount of how much an azimuth angle of the secondcompass is changed for the sampling period, ω_(g) indicates an angularvelocity of the gyro, Δt indicates a sampling time, ΔH_(1E) indicates adifference between the magnitude of a magnetic field of the firstcompass and the magnitude of the geomagnetic field, and ΔH_(2E)indicates a difference between the magnitude of a magnetic field of thesecond compass and the magnitude of the geomagnetic field, if a valueobtained by multiplying ΔH_(1E) by ΔH_(2E) is a negative number,determining a value obtained by calculating Formula 3 as a final compassazimuth angle θ_(c); if the value obtained by multiplying ΔH_(1E) byΔH_(2E) is a positive number, determining a value obtained bycalculating Formula 4 as the final compass azimuth angle θ_(c); and ifthe value obtained by multiplying ΔH_(1E) by ΔH_(2E) is 0, determining avalue obtained by calculating Formula 5 as the final compass azimuthangle θ_(c). The obtaining of the compass azimuth angle of the mobilebody by obtaining the differences between the angular velocities cancomprise: when Δθ_(c1) indicates the amount of how much an azimuth angleof the first compass is changed for a sampling period, Δθ_(c2) indicatesthe amount of how much an azimuth angle of the second compass is changedfor the sampling period, ω_(g) indicates an angular velocity of thegyro, and Δt indicates a sampling time, if the difference between anazimuth angle of the first compass θ_(c1) and an azimuth angle of thesecond compass θ_(c2) is not less than the third threshold value,checking whether or not to satisfy Formula 6; if Formula 6 is satisfied,determining the azimuth angle of the first compass θ_(c1) as the finalcompass azimuth angle θ_(c); if Formula 6 is not satisfied and Formula 7is satisfied, determining the azimuth angle of the second compass θ_(c2)as the final compass azimuth angle θ_(c); and if both sides of each ofFormula 6 and Formula 7 are the same, determining the value obtained bycalculating Formula 5 as the final compass azimuth angle, i.e., themoving direction angle of the mobile body, θ_(c).

$\begin{matrix}\left. \theta_{c}\leftarrow\frac{{\theta_{c\; 1}{{\Delta\; H_{2E}}}} + {\theta_{c2}{{\Delta\; H_{1E}}}}}{{{\Delta\; H_{1E}}} + {{\Delta\; H_{2E}}}} \right. & \left\lbrack {{Formula}\mspace{14mu} 3} \right\rbrack \\\left. \theta_{c}\leftarrow\frac{{\theta_{c\; 1}\Delta\; H_{2E}} - {\theta_{c\; 2}\Delta\; H_{1E}}}{{\Delta\; H_{1E}} - {\Delta\; H_{2E}}} \right. & \left\lbrack {{Formula}\mspace{14mu} 4} \right\rbrack \\\left. \theta_{c}\leftarrow\frac{\theta_{c\; 1} + \theta_{c\; 2}}{2} \right. & \left\lbrack {{Formula}\mspace{14mu} 5} \right\rbrack \\{{{\frac{\Delta\;\theta_{c\; 1}}{\Delta\; t} - \omega_{g}}} < {{\frac{\Delta\;\theta_{c\; 2}}{\Delta\; t} - \omega_{g}}}} & \left\lbrack {{Formula}\mspace{14mu} 6} \right\rbrack \\{{{\frac{\Delta\;\theta_{c\; 2}}{\Delta\; t} - \omega_{g}}} < {{\frac{\Delta\;\theta_{c\; 1}}{\Delta\; t} - \omega_{g}}}} & \left\lbrack {{Formula}\mspace{14mu} 7} \right\rbrack\end{matrix}$

The estimating of the moving direction of the mobile body in the casewhere each of the magnitude of the magnetic field of the first compassand the magnitude of the magnetic field of the second compass iscompared with the magnitude of the geomagnetic field and only one of thetwo differences is less than the first threshold value can comprise:determining an azimuth angle of the compass having the difference lessthan the first threshold value as the final compass azimuth angle.

The estimating of the moving direction of the mobile body in the casewhere each of the magnitude of the magnetic field of the first compassand the magnitude of the magnetic field of the second compass iscompared with the magnitude of the geomagnetic field and the twodifferences are not less than the first threshold value can comprise:calculating an angular velocity with respect to wheel velocities of themobile body (that is, an angular velocity obtained by the odometers)without using the compass information; obtaining a difference between agyro angular velocity of the mobile body and the wheel angular velocity;if the difference is less than a second threshold value, estimating anoptimum moving direction angle using a Kalman filter using a directionobtained by the odometer as a measurement input; and if the differenceis not less than the second threshold value, estimating the movingdirection angle by integrating the gyro angular velocity without usingthe Kalman filter. The estimating of the moving direction angle byintegrating the gyro angular velocity can further comprise: if theestimating is performed more than second threshold times within apredetermined time, ending the operation.

The method can further comprise: calculating an optimum moving directionestimation value of the mobile body by filtering a moving directionangle of the mobile body using the Kalman filter feedbacking an errorstatus.

According to another aspect of the present invention, there is provideda motion estimation system for a mobile body in which a gyro, odometers,and compasses are installed, the system comprising: a magnetic fieldcalculator calculating the magnitudes of magnetic fields of the mobilebody in which the compasses are installed; a magnetic field comparatorobtaining differences between the magnitudes of the magnetic fields andthe magnitude of the geomagnetic field and comparing the differenceswith the first threshold value; a geomagnetic region determinerdetermining whether a position of the mobile body belongs to a regionwhere the geomagnetism works according to the comparison result; and amoving direction estimator estimating a moving direction of the mobilebody by determining whether or not to use azimuth angles of thecompasses for direction estimation of the mobile body according to thedetermination result.

According to another aspect of the present invention, there is provideda computer readable medium having recorded thereon a computer readableprogram for performing the method described above.

BRIEF DESCRIPTION OF THE DRAWINGS

The above and other features and advantages of the present inventionwill become more apparent by describing in detail exemplary embodimentsthereof with reference to the attached drawings in which:

FIG. 1 shows a mobile body moving on a two-dimensional plane forestimating a position (x, y) and a direction θ;

FIG. 2 is a conceptual diagram of a sensor fusion system installed in amobile body for estimating a moving direction of the mobile bodyaccording to an embodiment of the present invention;

FIG. 3 is a block diagram of a motion estimation system for a mobilebody according to an embodiment of the present invention;

FIG. 4 is a flowchart of a motion estimation method for a mobile bodyaccording to an embodiment of the present invention;

FIG. 5 illustrates an example of obtaining a magnitude of a magneticfield from compass information;

FIG. 6 is a flowchart illustrating whether H₁ and H₂ obtained by a firstcompass and a second compass belong to S_(H);

FIG. 7 is a flowchart of a process of calculating a moving direction ofa mobile body using a weighted partition method;

FIG. 8 is a flowchart of a process of determining a moving direction ofthe mobile body in a case where not both differences between themagnitudes of the magnetic fields of the compasses and the magnitude ofthe geomagnetic field obtained in FIG. 4 are less than a first thresholdvalue;

FIG. 9 is a flowchart of a process of estimating a moving direction ofthe mobile body by checking angular velocities of a gyro and odometers;

FIG. 10 shows a position of a mobile body on a two-dimensional plane;and

FIG. 11 shows an error status measurement model.

DETAILED DESCRIPTION OF THE INVENTION

Hereinafter, the present invention will now be described more fully withreference to the accompanying drawings, in which embodiments of theinvention are shown.

Roll and pitch of a mobile body can be exactly calculated using anaccelerometer and a gyro. However, it is known that it is very difficultto obtain the yaw of a mobile body. In embodiments of the presentinvention, a moving direction of a mobile body is estimated using a newsensor fusion system, which can calculate pose information of the mobilebody by accurately and robustly estimating the moving direction of themobile body. That is, a position and an orientation of the mobile bodyis optimally estimated by combining absolute sensors and relativesensors such as compasses, a gyro and odometers, effectively dealingwith various uncertain environments such as magnetic fields, obstacles,and slippery floors, and obtaining a yaw angle of the mobile body.

FIG. 2 is a conceptual diagram of a sensor fusion system installed in amobile body for estimating a moving direction of the mobile bodyaccording to an embodiment of the present invention.

Referring to FIG. 2, the mobile body 200 includes two compasses 210 and220, a gyro 230, and two wheel encoders (odometers) 240 and 250.

The sensors have the following characteristics. The gyro 230 generatesan integration error in an integrating process required for calculatinga bias drift error and an angle value in a long-term. However, the gyro230 can detect a relatively accurate angle value in a short-term. Sincethe compasses 210 and 220 can provide an absolute azimuth angle in along-term, accurate information can be obtained using the compasses 210and 220. However, the compasses 210 and 220 can generates errors due tomagnetic disturbances existing in life environments in a short-term. Onthe other hand, the odometers 240 and 250 installed in driving parts(wheels in a case of a mobile body) of the mobile body show an unboundederror characteristic in a long-term since an error due to slippage andan error due to kinematic conditions, such as a wheel size and whetheralignment is performed, are accumulated. The odometers 240 and 250 alsohave such limitation that cannot deal with an unexpected situation suchas a bump collision and kidnapping in a short-term. However, theodometers 240 and 250 provide relatively accurate information duringmost of the time.

A direction and magnitude of a magnetic field can be obtained from thecompasses 210 and 220. An angular velocity can be detected using thegyro 230. An angular velocity of the mobile body can be obtained bydetecting rotation velocities of the wheels using the odometers 240 and250.

In embodiments of the present invention, a rule-based indirect Kalmanfilter is used for optimally estimating a position and an orientation ofthe mobile body by combining the absolute sensors and relative sensorssuch as compasses, a gyro, and odometers, effectively dealing withvarious uncertain environments such as magnetic fields, obstacles, andslippery floors, and obtaining a yaw angle of the mobile body.

FIG. 3 is a block diagram of a motion estimation system for a mobilebody according to an embodiment of the present invention.

Referring to FIG. 3, the motion estimation system for a mobile bodyincludes a magnetic field calculator 300, a magnetic field comparator310, a geomagnetic region determiner 320, and a moving directionestimator 330. A gyro, odometers, and compasses are installed in themobile body.

The mobile body obtains sensor data from the compasses, the gyro, andthe odometers. The magnitude of a magnetic field can be obtained fromcompass information. The magnetic field calculator 300 calculates themagnitudes of the magnetic fields of the mobile body in which thecompasses are installed. The magnetic field comparator 310 obtainsdifferences between the magnitudes of the magnetic fields and themagnitude of the geomagnetic field and compares the differences with afirst threshold value ε_(H). The geomagnetic region determiner 320determines whether a position of the mobile body belongs to a regionwhere the geomagnetism works according to the comparison result. Themoving direction estimator 330 estimates a moving direction of themobile body by determining whether or not to use a compass azimuth anglefor direction estimation of the mobile body according to thedetermination result. As the comparison result of differences betweenthe magnitudes of the magnetic fields obtained from the two compassesand the magnitude of the geomagnetic field, which is obtained by themagnetic field comparator 310, if at least one of the differences isless than the first threshold value ε_(H), an azimuth angle obtainedfrom the compass data is used for estimating the moving direction of themobile body. Otherwise, angular velocities of the gyro and the odometersare used for estimating the moving direction of the mobile body. Themotion estimation of the mobile body will now be described in moredetail with reference to the accompanying drawings.

FIG. 4 is a flowchart of a motion estimation method for a mobile bodyaccording to an embodiment of the present invention.

Sensor data of compasses, a gyro, and odometers is read in operation400. First, the magnitude of each magnetic field from compass data ofthe sensor data is obtained. FIG. 5 illustrates an example of obtainingthe magnitude of a magnetic field from compass information. Referring toFIG. 5, the magnitude of the magnetic field |H| can be obtained from anx-axis component magnetic field H_(x) and a y-axis component magneticfield H_(y), which can be obtained from each compass. Now, a differencebetween the magnitude of each magnetic field obtained from each compassand the magnitude of the geomagnetic field is defined as shown inFormula 1, where ΔH_(iE) indicates the difference, i indicates anidentifier of a compass, and H_(E) indicates the geomagnetic field.ΔH _(iE) ≡|H _(i) |−H _(E)|  [Formula 1]

Formula 2 is defined.If |ΔH _(iE)|<ε_(H) , H _(i) ∈S _(H)  [Formula 2]

where ε_(H) indicates a threshold value and S_(H) indicates a successregion, that is, a region where the geomagnetic field works.

After Formulas 1 and 2 are defined, H₁ and H₂ are obtained from a firstcompass and a second compass as shown in FIG. 6 in operation 600, and itis checked whether H₁ and H₂ belong to S_(H) in operation 410.

Each of the magnitude of the magnetic field of the first compass |H₁|and the magnitude of the magnetic field of the second compass |H₂| iscompared with the magnitude of the geomagnetic field |H_(E)|, and ifboth differences are less than the first threshold value ε_(H) inoperation 610, a final compass azimuth angle is calculated using aweighted partition method as shown in FIG. 7 in operation 420. Adifference between an azimuth angle of the first compass θ_(c1) and anazimuth angle of the second compass θ_(c2) is obtained in operation 700.If the azimuth angle difference is less than a third threshold valueε_(c), a compass azimuth angle of the mobile body is obtained by varyinga weight according to the differences between the magnitudes of themagnetic fields of the compasses and the magnitude of the geomagneticfield. That is, if a value obtained by multiplying ΔH_(1E) and ΔH_(2E)is a negative number in operation 710, a value obtained by Formula 3 isdetermined as the final compass azimuth angle θ_(c) in operation 720.

$\begin{matrix}\left. \theta_{c}\leftarrow\frac{{\theta_{c\; 1}{{\Delta\; H_{2E}}}} + {\theta_{c\; 2}{{\Delta\; H_{1E}}}}}{{{\Delta\; H_{1E}}} + {{\Delta\; H_{2E}}}} \right. & \left\lbrack {{Formula}\mspace{14mu} 3} \right\rbrack\end{matrix}$

If the value obtained by multiplying ΔH_(1E) and ΔH_(2E) obtained inoperation 610 is a positive number in operation 730, a value obtained byFormula 4 is determined as the final compass azimuth angle θ_(c) inoperation 740.

$\begin{matrix}\left. \theta_{c}\leftarrow\frac{{\theta_{c\; 1}\Delta\; H_{2E}} - {\theta_{c\; 2}\Delta\; H_{1E}}}{{\Delta\; H_{1E}} - {\Delta\; H_{2E}}} \right. & \left\lbrack {{Formula}\mspace{14mu} 4} \right\rbrack\end{matrix}$

If the value obtained by multiplying ΔH_(1E) and ΔH_(2E) obtained inoperation 610 is 0, a value obtained by Formula 5 is determined as thefinal compass azimuth angle θ_(c) in operation 750.

$\begin{matrix}\left. \theta_{c}\leftarrow\frac{\theta_{c\; 1} + \theta_{c\; 2}}{2} \right. & \left\lbrack {{Formula}\mspace{14mu} 5} \right\rbrack\end{matrix}$

On the other hand, if the difference between the azimuth angle of thefirst compass θ_(c1) and the azimuth angle of the second compass θ_(c2)is not less than the third threshold value ε_(c) in operation 700, it ischecked whether Formula 6 is satisfied in operation 760.

$\begin{matrix}{{{\frac{\Delta\;\theta_{c\; 1}}{\Delta\; t} - \omega_{g}}} < {{\frac{\Delta\;\theta_{c\; 2}}{\Delta\; t} - \omega_{g}}}} & \left\lbrack {{Formula}\mspace{14mu} 6} \right\rbrack\end{matrix}$

If Formula 6 is satisfied in operation 760, the azimuth angle of thefirst compass θ_(c1) is determined as the final compass azimuth angleθ_(c) in operation 770. If Formula 6 is not satisfied in operation 760and Formula 7 is satisfied in operation 780, the azimuth angle of thesecond compass θ_(c2) is determined as the final compass azimuth angleθ_(c) in operation 790.

$\begin{matrix}{{{\frac{\Delta\;\theta_{c\; 2}}{\Delta\; t} - \omega_{g}}} < {{\frac{\Delta\;\theta_{c\; 1}}{\Delta\; t} - \omega_{g}}}} & \left\lbrack {{Formula}\mspace{14mu} 7} \right\rbrack\end{matrix}$

If both sides of Formulas 6 and 7 are the same in operations 760 and780, the value obtained by Formula 5 is determined as the final compassazimuth angle θ_(c) as in operation 750.

In Formulas 3 through 7, Δθ_(c1) indicates the amount of how much anazimuth angle of the first compass is changed for a sampling period,Δθ_(c2) indicates the amount of how much an azimuth angle of the secondcompass is changed for the sampling period, ω_(g) indicates an angularvelocity of the gyro, Δt indicates a sampling time.

Operation 450 of FIG. 4 will be described in detail with reference toFIGS. 6 and 8. In FIG. 6, in a case where each of the magnitude of themagnetic field of the first compass |H₁| and the magnitude of themagnetic field of the second compass |H₂| is compared with the magnitudeof the geomagnetic field |H_(E)|, the difference between the magnitudeof the magnetic field of the first compass |H₁| and the magnitude of thegeomagnetic field |H_(E)| is less than the first threshold value ε_(H),and the difference between the magnitude of the magnetic field of thesecond compass |H₂| and the magnitude of the geomagnetic field |H_(E)|is not less than the first threshold value ε_(H) in operation 620, asshown in FIG. 8, the azimuth angle of the first compass Δθ_(c1) isdetermined as the final compass azimuth angle θ_(c) as in operation 800.

Also, in FIG. 6, in a case where each of the magnitude of the magneticfield of the first compass |H₁| and the magnitude of the magnetic fieldof the second compass |H₂| is compared with the magnitude of thegeomagnetic field |H_(E)|, the difference between the magnitude of themagnetic field of the first compass |H₁| and the magnitude of thegeomagnetic field |H_(E)| is not less than the first threshold valueε_(H), and the difference between the magnitude of the magnetic field ofthe second compass |H₂| and the magnitude of the geomagnetic field|H_(E)| is less than the first threshold value ε_(H) in operation 630,the azimuth angle of the second compass Δθ_(c2) is determined as thefinal compass azimuth angle θ_(c) as in operation 810.

An error status feedback Kalman filter is driven using the final compassazimuth angle θ_(c) obtained by the method described above in operation430, and an optimally estimated moving direction is obtained using theerror status feedback Kalman filter in operation 440.

On the other hand, in FIG. 6, in a case where each of the magnitude ofthe magnetic field of the first compass |H₁| and the magnitude of themagnetic field of the second compass |H₂| is compared with the magnitudeof the geomagnetic field |H_(E)| and both the differences are not lessthan the first threshold value ε_(H) in operations 410 and 630, compassdata is not used, and a difference between a gyro angular velocity andan odometer angular velocity is checked in operations 460 and 820.

Operations 460 through 495 of FIG. 4 will be described in detail withreference to FIG. 9. FIG. 9 is a flowchart of a process of estimating amoving direction of the mobile body by checking angular velocities ofthe gyro and odometers. First, an odometer angular velocity ω_(o) isobtained from the odometers as shown in Formula 8 in operation 900.

$\begin{matrix}{\omega_{o} = \frac{\left( {v_{r} - v_{t}} \right)}{l}} & \left\lbrack {{Formula}\mspace{14mu} 8} \right\rbrack\end{matrix}$

where V_(r) and v_(l) indicate wheel velocities and l indicates a treadlength, i.e., a distance between two wheels.

It is checked whether a difference between the gyro angular velocityω_(g) and the odometer angular velocity ω_(o) is less than a secondthreshold value ε_(ω) in operation 910, and if the difference is lessthan the second threshold value ε_(ω), the error status feedback Kalmanfilter is driven using the odometer data in operation 920, and anoptimally estimated moving direction is obtained using the error statusfeedback Kalman filter in operation 930.

Also, if the difference is not less than the second threshold valueε_(ω) as the check result of operation 910, the Kalman filter is notused, and the gyro angular velocity is integrated in operation 940 and asuboptimal moving direction is calculated in operation 950. A failureindex value is increased in operation 960, and if the failure indexvalue exceeds a fourth threshold value in operation 970, a warningsignal is issued and system operation is stopped in operation 980. Ifthe failure index value is equal to or less than the fourth thresholdvalue in operation 970, sensor data of the compasses, the gyro, and theodometers is read.

Modeling of odometers and a mobile body corresponding to an example ofthe mobile body can be performed as follows.

FIG. 10 shows a position of a mobile body on a two-dimensional plane.When t_(s) indicates a sampling time, the mobile body is modeled asshown in Formula 9.

$\begin{matrix}{\begin{pmatrix}{x\left( t_{k + 1} \right)} \\{y\left( t_{k + 1} \right)} \\{\theta\left( t_{k + 1} \right)}\end{pmatrix} = {\begin{pmatrix}{x\left( t_{k} \right)} \\{y\left( t_{k} \right)} \\{\theta\left( t_{k} \right)}\end{pmatrix} + \begin{pmatrix}{{\frac{\left( {{v_{r}\left( t_{k} \right)} + {v_{l}\left( t_{k} \right)}} \right)}{2} \cdot \Delta}\;{t \cdot \cos}\;{\theta\left( t_{k} \right)}} \\{{\frac{\left( {{v_{r}\left( t_{k} \right)} + {v_{l}\left( t_{k} \right)}} \right)}{2} \cdot \Delta}\;{t \cdot \sin}\;{\theta\left( t_{k} \right)}} \\{{\frac{\left( {{v_{r}\left( t_{k} \right)} - {v_{l}\left( t_{k} \right)}} \right)}{l} \cdot \Delta}\; t}\end{pmatrix}}} & \left\lbrack {{Formula}\mspace{14mu} 9} \right\rbrack\end{matrix}$

Also, when D_(l,r) indicates a diameter of a wheel, C_(e) indicates anencoder resolution, which is the number of pulses per one rotation ofthe wheel, n indicates a gear ratio, and N_(l,r) indicates the number ofpulses within a sampling period, the odometers are modeled as shown inFormula 10.

$\begin{matrix}{v_{l,r} = {\frac{\pi\; D_{l,r}}{n\; C_{e}} \cdot \frac{N_{l,r}}{t_{s}}}} & \left\lbrack {{Formula}\mspace{14mu} 10} \right\rbrack\end{matrix}$

Accordingly, an odometer angular velocity is represented as shown inFormula 11.

$\begin{matrix}{{\omega_{o}\left( t_{k} \right)} = \frac{\left( {{v_{r}\left( t_{k} \right)} - {v_{l}\left( t_{k} \right)}} \right)}{l}} & \left\lbrack {{Formula}\mspace{14mu} 11} \right\rbrack\end{matrix}$

Modeling of a gyro is performed as follows. First, when θ indicates amoving direction of the mobile body, b indicates a gyro drift rate,ω_(g) indicates a gyro output, n_(θ) indicates Gaussian white noisehaving distribution σ_(θ), and n_(b) indicates Gaussian white noisehaving distribution σ_(b), a gyro actual model can be represented asshown in Formula 12.

$\begin{matrix}\left\lbrack {{\begin{matrix}\begin{matrix}{\overset{.}{\theta} = {\omega_{g} + b + n_{g}}} \\{\overset{.}{b} = n_{b}}\end{matrix} & \Rightarrow\end{matrix}\begin{bmatrix}\overset{.}{\theta} \\\overset{.}{b}\end{bmatrix}} = {{\begin{bmatrix}0 & 1 \\0 & 0\end{bmatrix}\begin{bmatrix}\theta \\b\end{bmatrix}} + \begin{bmatrix}\omega_{g} \\0\end{bmatrix} + \begin{bmatrix}n_{\theta} \\n_{b}\end{bmatrix}}} \right. & \left\lbrack {{Formula}\mspace{14mu} 12} \right\rbrack\end{matrix}$

Also, when {tilde over (θ)} indicates an estimated moving direction and{tilde over (b)} indicates an estimated gyro drift rate (it is assumedthat {tilde over (b)} is a constant number), an output of a gyroestimation model, i.e., an integrator, can be represented as shown inFormula 13.

$\begin{matrix}{\begin{bmatrix}\overset{.}{\overset{\sim}{\theta}} \\\overset{.}{\overset{\sim}{b}}\end{bmatrix} = {{\begin{bmatrix}0 & 1 \\0 & 0\end{bmatrix}\begin{bmatrix}\overset{\sim}{\theta} \\\overset{\sim}{b}\end{bmatrix}} + \begin{bmatrix}\omega_{g} \\0\end{bmatrix}}} & \left\lbrack {{Formula}\mspace{14mu} 13} \right\rbrack\end{matrix}$

Modeling of an error status is performed as follows. First, the errorstatus is defined as shown in Formula 14.δθ(t)=θ(t)−{tilde over (θ)}(t)δb(t)=b(t)−{tilde over (b)}(t)  [Formula 14]

When θ_(m) indicates a measured moving direction and v indicatesGaussian white noise having distribution σ_(v), an external sensormeasurement model can be represented as shown in Formula 15. Here, anexternal sensor measurement input is selected with one of the compassfinal azimuth angle and the odometer angular velocity according to themethod described above.

$\begin{matrix}\begin{matrix}{{\theta_{m}(t)} = {{\theta(t)} + {\nu(t)}}} \\{{z(t)} = {{\theta_{m}(t)} - {\overset{\sim}{\theta}(t)}}} \\{\mspace{34mu}{= {\left( {{\theta(t)} + {\nu(t)}} \right) - \left( {{\theta(t)} - {\delta\;{\theta(t)}}} \right)}}} \\{\mspace{34mu}{= {{\delta\;{\theta(t)}} + {\nu(t)}}}} \\{\mspace{34mu}{= {\left. {{\begin{bmatrix}1 & 0\end{bmatrix}\begin{bmatrix}{\delta\;{\theta(t)}} \\{\delta\;{b(t)}}\end{bmatrix}} + {\nu(t)}}\Rightarrow{z(t)} \right. = {{{Hx}(t)} + {\nu(t)}}}}}\end{matrix} & \left\lbrack {{Formula}\mspace{14mu} 15} \right\rbrack\end{matrix}$

An error status model is obtained as shown in Formula 16 by subtractingFormula 12 from Formula 13.

$\begin{matrix}{\begin{bmatrix}{\delta\;{\overset{.}{\theta}(t)}} \\{\delta\;{\overset{.}{b}(t)}}\end{bmatrix} = {\left. {{\begin{bmatrix}0 & 1 \\0 & 0\end{bmatrix}\begin{bmatrix}{\delta\;{\theta(t)}} \\{\delta\;{b(t)}}\end{bmatrix}} + \begin{bmatrix}{n_{\theta}(t)} \\{n_{b}(t)}\end{bmatrix}}\mspace{14mu}\Rightarrow{\overset{.}{x}(t)} \right. = {{\overset{\Cup}{A}\;{x(t)}} + {n(t)}}}} & \left\lbrack {{Formula}\mspace{14mu} 16} \right\rbrack\end{matrix}$

FIG. 11 shows an error status measurement model.

The above model can be represented as a discrete model as follows. Adiscrete error status can be defined as shown in Formula 17.δθ(t _(k))≡θ(t _(k))−{tilde over (θ)}(t _(k)), δb(t _(k))≡b(t_(k))−{tilde over (b)}(t _(k))  [Formula 17]

A discrete form of Formula 12 can be represented as shown in Formula 18.

$\begin{matrix}{{\begin{bmatrix}{\theta\left( t_{k + 1} \right)} \\{b\left( t_{k + 1} \right)}\end{bmatrix} = {{\begin{bmatrix}1 & {\Delta\; t} \\0 & 1\end{bmatrix}\begin{bmatrix}{\theta\left( t_{k} \right)} \\{b\left( t_{k} \right)}\end{bmatrix}} + \begin{bmatrix}{\Delta\;{t \cdot {\omega_{g}\left( t_{k} \right)}}} \\0\end{bmatrix} + \begin{bmatrix}{n_{\theta}\left( t_{k} \right)} \\{n_{b}\left( t_{k} \right)}\end{bmatrix}}}{{\Delta\; t} \equiv {t_{k + 1} - t_{k}}}} & \left\lbrack {{Formula}\mspace{14mu} 18} \right\rbrack\end{matrix}$

A discrete form of Formula 13 can be represented as shown in Formula 19.

$\begin{matrix}{\begin{bmatrix}{\overset{\sim}{\theta}\left( t_{k + 1} \right)} \\{\overset{\sim}{b}\left( t_{k + 1} \right)}\end{bmatrix} = {{\begin{bmatrix}1 & {\Delta\; t} \\0 & 1\end{bmatrix}\;\begin{bmatrix}{\overset{\sim}{\theta}\left( t_{k} \right)} \\{\overset{\sim}{b}\left( t_{k} \right)}\end{bmatrix}} + \begin{bmatrix}{\Delta\;{t \cdot {\omega_{g}\left( t_{k} \right)}}} \\0\end{bmatrix}}} & \left\lbrack {{Formula}\mspace{14mu} 19} \right\rbrack\end{matrix}$

Formula 20 can be derived from Formulas 18 and 19.

$\begin{matrix}{\begin{bmatrix}{\delta\;{\theta\left( t_{k + 1} \right)}} \\{\delta\;{b\left( t_{k + 1} \right)}}\end{bmatrix} = {\left. {{\begin{bmatrix}1 & {\Delta\; t} \\0 & 1\end{bmatrix}\;\begin{bmatrix}{\delta\;{\theta\left( t_{k} \right)}} \\{\delta\;{b\left( t_{k} \right)}}\end{bmatrix}} + \begin{bmatrix}{n_{\theta}\left( t_{k} \right)} \\{n_{b}\left( t_{k} \right)}\end{bmatrix}}\mspace{14mu}\Rightarrow{x\left( t_{k + 1} \right)} \right. = {{A\;{x\left( t_{k} \right)}} + {n\left( t_{k} \right)}}}} & \left\lbrack {{Formula}\mspace{14mu} 20} \right\rbrack\end{matrix}$

A discrete form of Formula 15 can be represented as shown in Formula 21.

$\begin{matrix}\begin{matrix}{{z\left( t_{k} \right)} = {{\theta_{m}\left( t_{k} \right)} - {\overset{\sim}{\theta}\left( t_{k} \right)}}} & {\left. \Rightarrow{z\left( t_{k} \right)} \right. = {{{Hx}\left( t_{k} \right)} + {\nu\left( t_{k} \right)}}} \\{= {{\delta\;{\theta\left( t_{k} \right)}} + {\nu\left( t_{k} \right)}}} & \; \\{= {{\begin{bmatrix}1 & 0\end{bmatrix}\begin{bmatrix}{\delta\;{\theta\left( t_{k} \right)}} \\{\delta\;{b\left( t_{k} \right)}}\end{bmatrix}} + {\nu\left( t_{k} \right)}}} & \;\end{matrix} & \left\lbrack {{Formula}\mspace{14mu} 21} \right\rbrack\end{matrix}$

A discrete error status Kalman filter is described in detail as follows.Initial conditions can be represented as in Formulas 22 and 23. Formula22 represents a Gaussian random variable (RV), whose average value isbasically 0, and Formula 23 represents distribution of the initialvalues θ and b.

$\begin{matrix}{{x\left( t_{0} \right)} = \begin{bmatrix}0 & 0\end{bmatrix}^{T}} & \left\lbrack {{Formula}\mspace{14mu} 22} \right\rbrack \\{{P\left( t_{0} \right)} = \begin{bmatrix}\sigma_{\theta}^{2} & 0 \\0 & \sigma_{b}^{2}\end{bmatrix}} & \left\lbrack {{Formula}\mspace{14mu} 23} \right\rbrack\end{matrix}$

A discrete form of the Gaussian noise can be represented as shown inFormula 24.

$\begin{matrix}{{\begin{matrix}{Q = \begin{bmatrix}\sigma_{n_{\theta}}^{2} & 0 \\0 & \sigma_{n_{b}}^{2}\end{bmatrix}} & \Leftarrow & {{E\left\{ {\begin{bmatrix}{n_{\theta}\left( t_{i} \right)} \\{n_{b}\left( t_{i} \right)}\end{bmatrix}\begin{bmatrix}{n_{\theta}\left( t_{j} \right)} & {n_{b}\left( t_{j} \right)}\end{bmatrix}} \right\}} =}\end{matrix}{Q\left( t_{i} \right)}\;{\delta\left( {t_{i} - t_{j}} \right)}}{R = \sigma_{v}^{2}}} & \left\lbrack {{Formula}\mspace{14mu} 24} \right\rbrack\end{matrix}$

Discrete forms according to temporal updates can be represented as shownin Formulas 25 and 26.

$\begin{matrix}{\begin{bmatrix}{\delta\;{\hat{\theta}\left( t_{k + 1}^{-} \right)}} \\{\delta\;{\hat{b}\left( t_{k + 1}^{-} \right)}}\end{bmatrix} = {\begin{bmatrix}1 & {\Delta\; t} \\0 & 1\end{bmatrix}\begin{bmatrix}{\delta\;{\hat{\theta}\left( t_{k} \right)}} \\{\delta\;{\hat{b}\left( t_{k} \right)}}\end{bmatrix}}} & \left\lbrack {{Formula}\mspace{14mu} 25} \right\rbrack \\\begin{matrix}{{P\left( t_{k + 1}^{-} \right)} = {{{{{AP}\left( t_{k} \right)}A^{T}} + Q} = {{\begin{bmatrix}1 & {\Delta\; t} \\0 & 1\end{bmatrix} \cdot {P\left( t_{k} \right)} \cdot \begin{bmatrix}1 & 0 \\{\Delta\; t} & 1\end{bmatrix}} + Q}}} \\{\mspace{76mu}{= \begin{bmatrix}{{P_{11}\left( t_{k} \right)} + {2\Delta\;{{tP}_{12}\left( t_{k} \right)}} + {\left( {\Delta\; t} \right)^{2}{P_{22}\left( t_{k} \right)}} + \sigma_{n_{\theta}}^{2}} & {{P_{12}\left( t_{k} \right)} + {\Delta\;{t \cdot {P_{22}\left( t_{k} \right)}}}} \\{{P_{12}\left( t_{k} \right)} + {\Delta\;{t \cdot {P_{22}\left( t_{k} \right)}}}} & {{P_{22}\left( t_{k} \right)} + \sigma_{n_{b}}^{2}}\end{bmatrix}}}\end{matrix} & \left\lbrack {{Formula}\mspace{14mu} 26} \right\rbrack\end{matrix}$

Discrete forms according to measurement updates can be represented asshown in Formulas 27, 28 and 29.

$\begin{matrix}\begin{matrix}{{K\left( t_{k + 1} \right)} = {{P\left( t_{k + 1}^{-} \right)}{H^{T}\left( {{{{HP}\left( t_{k + 1}^{-} \right)}H^{T}} + R} \right)}^{- 1}}} \\{\mspace{79mu}{= {\frac{1}{{P_{11}\left( t_{k + 1}^{-} \right)} + \sigma_{v}^{2}}\begin{bmatrix}{P_{11}\left( t_{k + 1}^{-} \right)} \\{P_{21}\left( t_{k + 1}^{-} \right)}\end{bmatrix}}}}\end{matrix} & \left\lbrack {{Formula}\mspace{14mu} 27} \right\rbrack \\\begin{matrix}{\begin{bmatrix}{\delta\;{\hat{\theta}\left( t_{k + 1} \right)}} \\{\delta\;{\hat{b}\left( t_{k + 1} \right)}}\end{bmatrix} = {\begin{bmatrix}{\delta\;{\hat{\theta}\left( t_{k + 1}^{-} \right)}} \\{\delta\;{\hat{b}\left( t_{k + 1}^{-} \right)}}\end{bmatrix} + {{K\left( t_{k + 1} \right)} \cdot \left( {{z\left( t_{k + 1} \right)} - {H \cdot \begin{bmatrix}{\delta\;{\hat{\theta}\left( t_{k + 1}^{-} \right)}} \\{\delta\;{\hat{b}\left( t_{k + 1}^{-} \right)}}\end{bmatrix}}} \right)}}} \\{\mspace{115mu}{= {{\begin{bmatrix}1 & {\Delta\; t} \\0 & 1\end{bmatrix}\begin{bmatrix}{\delta\;{\hat{\theta}\left( t_{k} \right)}} \\{\delta\;{\hat{b}\left( t_{k} \right)}}\end{bmatrix}} + {\begin{bmatrix}{K_{1}\left( t_{k + 1} \right)} \\{K_{2}\left( t_{k + 1} \right)}\end{bmatrix} \cdot \left( {{z\left( t_{k + 1} \right)} - {\delta\;{\hat{\theta}\left( t_{k + 1}^{-} \right)}}} \right)}}}}\end{matrix} & \left\lbrack {{Formula}\mspace{14mu} 28} \right\rbrack \\\begin{matrix}{{P\left( t_{k + 1} \right)} = {{P\left( t_{k + 1}^{-} \right)} - {{K\left( t_{k + 1} \right)} \cdot H \cdot {P\left( t_{k + 1}^{-} \right)}}}} \\{\mspace{76mu}{= \begin{bmatrix}{\left( {1 - {K_{1}\left( t_{k + 1} \right)}} \right){P_{11}\left( t_{k + 1}^{-} \right)}} & {\left( {1 - {K_{1}\left( t_{k + 1} \right)}} \right){P_{12}\left( t_{k + 1}^{-} \right)}} \\{{P_{12}\left( t_{k + 1}^{-} \right)} - {{K_{2}\left( t_{k + 1} \right)}{P_{11}\left( t_{k + 1}^{-} \right)}}} & {{P_{22}\left( t_{k + 1}^{-} \right)} - {{K_{2}\left( t_{k + 1} \right)}{P_{12}\left( t_{k + 1}^{-} \right)}}}\end{bmatrix}}}\end{matrix} & \left\lbrack {{Formula}\mspace{14mu} 29} \right\rbrack\end{matrix}$

Formula 30 can be derived from Formula 19, and Formula 31 can be derivedfrom Formula 28.

$\begin{matrix}{\begin{bmatrix}{\overset{\sim}{\theta}\left( t_{k + 1} \right)} \\{\overset{\sim}{b}\left( t_{k + 1} \right)}\end{bmatrix} = {{\begin{bmatrix}1 & {\Delta\; t} \\0 & 1\end{bmatrix}\begin{bmatrix}{\overset{\sim}{\theta}\left( t_{k} \right)} \\{\overset{\sim}{b}\left( t_{k} \right)}\end{bmatrix}} + \begin{bmatrix}{\Delta\;{t \cdot {\omega_{g}\left( t_{k} \right)}}} \\0\end{bmatrix}}} & \left\lbrack {{Formula}\mspace{14mu} 30} \right\rbrack \\{\begin{bmatrix}{\delta\;{\hat{\theta}\left( t_{k + 1} \right)}} \\{\delta\;{\hat{b}\left( t_{k + 1} \right)}}\end{bmatrix} = {{\begin{bmatrix}1 & {\Delta\; t} \\0 & 1\end{bmatrix}\begin{bmatrix}{\delta\;{\hat{\theta}\left( t_{k} \right)}} \\{\delta\;{\hat{b}\left( t_{k} \right)}}\end{bmatrix}} + {\begin{bmatrix}{K_{1}\left( t_{k + 1} \right)} \\{K_{2}\left( t_{k + 1} \right)}\end{bmatrix} \cdot}}} & \left\lbrack {{Formula}\mspace{14mu} 31} \right\rbrack \\{\mspace{146mu}\left( {{z\left( t_{k + 1} \right)} - {\delta\;{\hat{\theta}\left( t_{k + 1}^{-} \right)}}} \right)} & \;\end{matrix}$

When it is defined that {circumflex over (θ)}≡{tilde over(θ)}+δ{circumflex over (θ)}, {circumflex over (b)}≡{tilde over(b)}+δ{circumflex over (b)}, Formula 32 can be obtained from Formula 30and Formula 31.

$\begin{matrix}{\begin{bmatrix}{\hat{\theta}\left( t_{k + 1} \right)} \\{\hat{b}\left( t_{k + 1} \right)}\end{bmatrix} = {{\begin{bmatrix}1 & {\Delta\; t} \\0 & 1\end{bmatrix}\begin{bmatrix}{\hat{\theta}\left( t_{k} \right)} \\{\hat{b}\left( t_{k} \right)}\end{bmatrix}} + \begin{bmatrix}{\Delta\;{t \cdot {\omega_{g}\left( t_{k} \right)}}} \\0\end{bmatrix} +}} & \left\lbrack {{Formula}\mspace{14mu} 32} \right\rbrack \\{\mspace{135mu}{\begin{bmatrix}{K_{1}\left( t_{k + 1} \right)} \\{K_{2}\left( t_{k + 1} \right)}\end{bmatrix} \cdot \left( {{z\left( t_{k + 1} \right)} - {\delta\;{\hat{\theta}\left( t_{k + 1}^{-} \right)}}} \right)}} & \; \\{\mspace{104mu}{= {{\begin{bmatrix}1 & {\Delta\; t} \\0 & 1\end{bmatrix}\begin{bmatrix}{\hat{\theta}\left( t_{k} \right)} \\{\hat{b}\left( t_{k} \right)}\end{bmatrix}} + \begin{bmatrix}{\Delta\;{t \cdot {\omega_{g}\left( t_{k} \right)}}} \\0\end{bmatrix} +}}} & \; \\{\mspace{135mu}{\begin{bmatrix}{K_{1}\left( t_{k + 1} \right)} \\{K_{2}\left( t_{k + 1} \right)}\end{bmatrix} \cdot \left( {{\theta_{m}\left( t_{k + 1} \right)} - {\overset{\sim}{\theta}\left( t_{k + 1} \right)} -} \right.}} & \; \\\left. \mspace{140mu}{\delta\;{\hat{\theta}\left( t_{k + 1}^{-} \right)}} \right) & \;\end{matrix}$

The initial conditions are as shown in Formula 33.{circumflex over (θ)}(t ₀)=θ_(m)(t ₀) {tilde over (θ)}(t ₀)=θ_(m)(t ₀)δθ(t ₀)=0{circumflex over (b)}(t ₀)={tilde over (b)}(t ₀)=δb(t ₀)=0  [Formula 33]

Formula 32 is a representative formula of the error status Kalman filterused in an embodiment of the present invention. According to the methoddescribed above, an optimal moving direction estimation value{circumflex over (θ)}(t_(k+1)) can be obtained by selecting one of thecompass azimuth angle and the odometer angular velocity as themeasurement input and substituting the measurement input for θ_(m) ofFormula 32.

The invention can be embodied as computer readable codes on a computer(including all apparatuses having a information processing function)readable recording medium. The computer readable recording medium is anydata storage device that can store data which can be thereafter read bya computer system. Examples of the computer readable recording mediuminclude read-only memory (ROM), random-access memory (RAM), CD-ROMs,magnetic tapes, floppy disks, optical data storage devices.

As described above, in a sensor fusion method according to an embodimentof the present invention, sensors with a high specification are notnecessary. Accordingly, a high performance can be realized withlow-price sensors. Also, various unexpected situations that can be dealtwith when a mobile body is moved and various kinds of error sources canbe estimated. Furthermore, various typical limits included in the Kalmanfilter, such as that exact information of error characteristics of thesensors is not necessary and that bias, colored noise, and nonsystematicerrors can be dealt with, can be overcome. As an ancillary effect, asystem fault sensing function can be realized.

Since 3-dimensional angle information is robustly provided byeffectively obtaining a yaw angle according to an embodiment of thepresent invention, the present invention can be applied to all fieldsrequiring estimation of an absolute position and orientation. Theapplicable fields are as follows: autonomic traveling vehicles,intelligent vehicles, car navigation, medical robots, virtual reality,entertainment, unmanned planes, and personal navigation.

While the present invention has been particularly shown and describedwith reference to exemplary embodiments thereof, it will be understoodby those of ordinary skill in the art that various changes in form anddetails may be made therein without departing from the spirit and scopeof the present invention as defined by the following claims.

1. A motion estimation method for a mobile body, the method comprising:obtaining magnetic field information from compass information of themobile body; comparing each of the magnitude of a magnetic field of afirst compass and the magnitude of a magnetic field of a second compasswith the magnitude of a geomagnetic field, and determining whether aposition of the mobile body belongs to a specific region according tothe comparison result; and determining a moving direction of the mobilebody by determining whether a compass azimuth angle is to be used inestimating the moving direction of the mobile body based on thedetermination of whether the position of the mobile body belongs to thespecific region.
 2. The method of claim 1, wherein the specific regionis a region in which the geomagnetism dominates.
 3. The method of claim2, wherein the determining of whether a position of the mobile bodybelongs to a specific region comprises: dividing the comparison resultsinto a case where both differences are less than a first thresholdvalue, a case where only one of the two differences is less than thefirst threshold value, and a case where the two differences are not lessthan the first threshold value, and determining whether the position ofthe mobile body belongs to the specific region according to thecomparison results.
 4. The method of claim 3, wherein the determining ofthe moving direction of the mobile body comprises: if it is determinedthat the position of the mobile body belongs to the specific region,obtaining a final compass azimuth angle and estimating a heading angleusing a Kalman filter using the final compass azimuth angle as ameasurement input; and if it is determined that the position of themobile body to the specific region, comparing an angular velocity of agyro and an angular velocity of an odometer, if the difference is lessthan a second threshold value, estimating a heading direction using theKalman filter using a moving direction obtained by the odometer as ameasurement input, and if the difference is not less than the secondthreshold value, estimating the moving direction by integrating theangular velocity of the gyro.
 5. The method of claim 3, wherein thedetermining of the moving direction of the mobile body in the case whereeach of the magnitude of the magnetic field of the first compass and themagnitude of the magnetic field of the second compass is compared withthe magnitude of the geomagnetic field and the two differences are lessthan the first threshold value comprises: obtaining a difference betweenan azimuth angle of the first compass and an azimuth angle of the secondcompass; if the difference between the azimuth angles is less than athird threshold value, obtaining a compass azimuth angle of the mobilebody by varying a weight according to differences between the magnitudesof the magnetic fields of the two compasses and the magnitude of thegeomagnetic field; and if the difference between the azimuth angles isnot less than the third threshold value, obtaining a difference betweenan angular velocity of the gyro with respect to the mobile body and eachangular velocity with respect to the azimuth angles of the compasses andobtaining the compass azimuth angle of the mobile body according to themagnitudes of the differences.
 6. The method of claim 5, wherein theobtaining of the compass azimuth angle of the mobile body by varying aweight comprises: when Δθ_(c1) indicates the amount of how much anazimuth angle of the first compass is changed for a sampling period,Δθ_(c2) indicates the amount of how much an azimuth angle of the secondcompass is changed for the sampling period, ω_(g) indicates an angularvelocity of the gyro, Δt indicates a sampling time, ΔH_(1E) indicates adifference between the magnitude of a magnetic field of the firstcompass and the magnitude of the geomagnetic field, and ΔH_(2E)indicates a difference between the magnitude of a magnetic field of thesecond compass and the magnitude of the geomagnetic field, if a valueobtained by multiplying ΔH_(1E) by ΔH_(2E) is a negative number,determining a final compass azimuth angle θ_(c) as follows;$\left. \theta_{c}\leftarrow\frac{{\theta_{c\; 1}{{\Delta\; H_{2E}}}} + {\theta_{c\; 2}{{\Delta\; H_{1E}}}}}{{{\Delta\; H_{1E}}} + {{\Delta\; H_{2E}}}} \right.$if the value obtained by multiplying ΔH_(1E) by ΔH_(2E) is a positivenumber, determining the final compass azimuth angle θ_(c) as follows;$\left. \theta_{c}\leftarrow\frac{{\theta_{c\; 1}\Delta\; H_{2E}} - {\theta_{c\; 2}\Delta\; H_{1E}}}{{\Delta\; H_{1E}} - {\Delta\; H_{2E}}} \right.$and if the value obtained by multiplying ΔH_(1E) by ΔH_(2E) is 0,determining the final compass azimuth angle θ_(c) as follows$\left. \theta_{c}\leftarrow\frac{\theta_{c\; 1} + \theta_{c\; 2}}{2} \right.$7. The method of claim 5, wherein the obtaining of the compass azimuthangle of the mobile body by obtaining the differences between theangular velocities comprises: when Δθ_(C1) indicates the amount of howmuch an azimuth angle of the first compass is changed for a samplingperiod, Δθ_(c2) indicates the amount of how much an azimuth angle of thesecond compass is changed for the sampling period, ω_(g) indicates anangular velocity of the gyro, and Δt indicates a sampling time, if thedifference between an azimuth angle of the first compass θ_(c1) and anazimuth angle of the second compass θ_(C2) is not less than the thirdthreshold value, if${{\frac{\Delta\;\theta_{c\; 1}}{\Delta\; t} - \omega_{g}} < {{\frac{\Delta\;\theta_{c\; 2}}{\Delta\; t} - \omega_{g}}}}$is satisfied, determining the azimuth angle of the first compass θ_(c1)as the final compass azimuth angle θ_(c); if${{\frac{\Delta\;\theta_{c\; 1}}{\Delta\; t} - \omega_{g}}} < {{\frac{\Delta\;\theta_{c\; 2}}{\Delta\; t} - \omega_{g}}}$is not satisfied and${{\frac{\Delta\;\theta_{c\; 2}}{\Delta\; t} - \omega_{g}}} < {{\frac{\Delta\;\theta_{c\; 1}}{\Delta\; t} - \omega_{g}}}$is satisfied, determining the azimuth angle of the second compass θ_(c2)as the final compass azimuth angle θ_(c); and if Δθ_(c1) and Δθ_(c2) areequal to each other, determining the moving direction angle of themobile body, θ_(c) as the final compass azimuth angle as follows$\left. \theta_{c}\leftarrow{\frac{\theta_{c\; 1} + \theta_{c\; 2}}{2}.} \right.$8. The method of claim 3, wherein the determining of the movingdirection of the mobile body in the case where each of the magnitude ofthe magnetic field of the first compass and the magnitude of themagnetic field of the second compass is compared with the magnitude ofthe geomagnetic field and only one of the two differences is less thanthe first threshold value comprises: determining an azimuth angle of thecompass having the difference less than the first threshold value as thefinal compass azimuth angle.
 9. The method of claim 8, furthercomprising calculating an optimum moving direction estimation value ofthe mobile body by filtering a moving direction angle of the mobile bodyusing the Kalman filter feedbacking an error status.
 10. The method ofclaim 3, wherein the determining of the moving direction of the mobilebody in the case where each of the magnitude of the magnetic field ofthe first compass and the magnitude of the magnetic field of the secondcompass is compared with the magnitude of the geomagnetic field and thetwo differences are not less than the first threshold value comprises:calculating an angular velocity with respect to wheel velocities of themobile body without using the compass information; obtaining adifference between a gyro angular velocity of the mobile body and thewheel angular velocity; if the difference is less than a secondthreshold value, estimating an optimum moving direction angle using theKalman filter using a direction obtained by the odometer as ameasurement input; and if the difference is not less than the secondthreshold value, estimating the moving direction angle by integratingthe gyro angular velocity without using the Kalman filter.
 11. Themethod of claim 10, wherein the determining of the moving directionangle by integrating the gyro angular velocity further comprises: if theestimating is performed more times than a fourth threshold value withina predetermined time, then the estimating is ended.
 12. The method ofclaim 5, further comprising calculating an optimum moving directionestimation value of the mobile body by filtering a moving directionangle of the mobile body using the Kalman filter feedbacking an errorstatus.
 13. A computer readable medium having recorded thereon acomputer readable program for performing the method of claim
 1. 14. Amotion estimation system for a mobile body in which a gyro, odometers,and compasses are installed, the system comprising: a magnetic fieldcalculator calculating the magnitudes of magnetic fields of a firstcompass and a second compass of the mobile body; a magnetic fieldcomparator obtaining differences between the magnitudes of the magneticfields of the first and second compasses and the magnitude of ageomagnetic field, and comparing the differences with a first thresholdvalue; a geomagnetic region determiner determining whether a position ofthe mobile body belongs to a region where the geomagnetism worksaccording to the comparison result; and a moving direction estimatorestimating a moving direction of the mobile body by determining whetheror not to use azimuth angles of the compasses for direction estimationof the mobile body according to the determination result.